/*
 * centerline_generator.c
 *
 *  Created on: 2025年7月1日
 *      Author: zhuyuanchen
 */

#include "centerline_generator.h"
#include <math.h>
#include <string.h>
#include <float.h>
#include <limits.h>
#include "zf_driver_gpio.h"
#include "open_count.h"
int TEST = STATE_NORMAL_TRACKING;
CornerDirection corner_left = 0;
CornerDirection corner_right = 0;
runstate runstate_display = 0;
int left_count_display = 0, right_count_display = 0;
int straight_left = 0, straight_right = 0;
int centerline_width_count = 0;
extern CenterlineGenerator g_centerline_gen;
//================================================================
// 私有辅助函数声明 (内部使用)
//================================================================

// 更新短边线查找表
static void update_short_line_lookup_map(CenterlineGenerator* instance, Point* short_line, int short_count);

// 稳定法线计算 (核心)
static bool calculate_stable_normal(Point* line, int count, int index, Point_f* out_normal);

// 更新宽度滤波器
static void update_width_filter(CenterlineGenerator* instance, float new_width);

static int GenerateCenterline_Single(
    CenterlineGenerator* instance,
    Point* single_line, int line_count,
    bool is_left_line,
    Point* out_centerline
);

static int GenerateCenterline_Dual(
    CenterlineGenerator* instance,
    Point* left_line, int left_count,
    Point* right_line, int right_count,
    Point* out_centerline
);

int find_first_corner_idx(
    const Point* line,
    int point_count,
    int start_idx,
    int k,
    float cos_theta_thresh);

bool is_line_straight_by_endpoints(
    const Point* line,
    int num,
    int minnum,
    int maxnum,
    float mse_thresh);

CornerDirection find_first_corner(
    const Point* line,
    int point_count,
    int start_idx,
    int k,
    float cos_theta_thresh,
    int* out_corner_idx);

int check_zebra_crossing_at_line(
    const uint8_t binarized_image[IMAGE_HEIGHT][IMAGE_WIDTH],
    int center_row,
    int row_offset,
    int threshold
);
//================================================================
// 公共函数定义
//================================================================
// 中线循迹部分
void CenterlineGenerator_Init(CenterlineGenerator* instance, float initial_track_width)
{
    instance->filtered_width = initial_track_width;
    instance->width_buffer_index = 0;
    for (int i = 0; i < WIDTH_FILTER_SIZE; ++i) {
        instance->width_buffer[i] = initial_track_width;
    }
    instance->is_left_corner_locked = false;
    instance->is_right_corner_locked = false;
    instance->current_state = STATE_NORMAL_TRACKING;
    instance->centerline = CENTERLINE_NORMAL;
    instance->centerline = STRAIGHT_ROAD;
    // 确保查找表在开始时是干净的
    memset(instance->short_line_lookup_map, 0, sizeof(instance->short_line_lookup_map));
}

int GenerateCenterline(
    CenterlineGenerator* instance,
    Point* left_line, int left_count,
    Point* right_line, int right_count,
    Point* out_centerline
)
{
    // --- 步骤 1: 定义算法所需常量 ---
    const int MIN_VALID_POINTS = 2 * NORMAL_FIT_NEIGHBORHOOD + 1;
    // 【修改】定义迟滞比较法的双阈值
    const int CORNER_DETECT_K = 5;
    const float CORNER_COS_THETA_LOCK = 0.46f;    // 锁定阈值
    const float CORNER_COS_THETA_RELEASE = 0.57f; // 释放阈值

    // --- 步骤 2: 【重构】高效稳定的直角检测与状态更新 ---

    // ## 处理左边线 ##
    int left_lock_idx = -1;
    CornerDirection left_corner_direction = NO_CORNER;
    if (left_count > CORNER_DETECT_K * 2) {
        // 只调用一次find_first_corner_idx来找到满足“锁定”条件的拐点
        left_corner_direction = find_first_corner(left_line, left_count, CORNER_DETECT_K, CORNER_DETECT_K, CORNER_COS_THETA_LOCK, &left_lock_idx);
    }

    if (instance->is_left_corner_locked) {
        // 如果已锁定，需要一个强信号来释放。
        // 信号是：用宽松的阈值都找不到任何拐点了。
        int release_check_idx = -1;
        find_first_corner(left_line, left_count, CORNER_DETECT_K, CORNER_DETECT_K, CORNER_COS_THETA_RELEASE, &release_check_idx);
        if (release_check_idx == -1) {
            instance->is_left_corner_locked = false;
        }
    } else {
        // 如果未锁定，需要一个强信号来锁定。
        // 信号是：找到了满足严格锁定条件的拐点。
        if (left_lock_idx != -1) {
            instance->is_left_corner_locked = true;
        }
    }
    corner_left = left_corner_direction;
    // ## 处理右边线 (逻辑同上) ##
    int right_lock_idx = -1;
    CornerDirection right_corner_direction = NO_CORNER;
    if (right_count > CORNER_DETECT_K * 2) {
        right_corner_direction = find_first_corner(right_line, right_count, CORNER_DETECT_K, CORNER_DETECT_K, CORNER_COS_THETA_LOCK, &right_lock_idx);
    }

    if (instance->is_right_corner_locked) {
        int release_check_idx= -1;
        find_first_corner(right_line, right_count, CORNER_DETECT_K, CORNER_DETECT_K, CORNER_COS_THETA_RELEASE, &release_check_idx);
        if (release_check_idx == -1) {
            instance->is_right_corner_locked = false;
        }
    } else {
        if (right_lock_idx != -1) {
            instance->is_right_corner_locked = true;
        }
    }
    corner_right = right_corner_direction;
    // --- 步骤 3: 根据更新后的稳定状态执行截断 ---
    int final_left_count = left_count;
    if (instance->is_left_corner_locked && left_lock_idx != -1) {
        final_left_count = left_lock_idx + 1;
    }

    int final_right_count = right_count;
    if (instance->is_right_corner_locked && right_lock_idx != -1) {
        final_right_count = right_lock_idx + 1;
    }

    bool is_straight_left = is_line_straight_by_endpoints(left_line, left_count, 25, 40, 1.5f);
    bool is_straight_right = is_line_straight_by_endpoints(right_line, right_count, 25, 40, 1.5f);

    bool is_straight_left_road = is_line_straight_by_endpoints(left_line, final_left_count, 40, 70, 0.6f);
    bool is_straight_right_road = is_line_straight_by_endpoints(right_line, final_right_count, 40, 70, 0.6f);
    if ((is_straight_left_road || is_straight_right_road) &&
            instance->current_state != STATE_ROUNDLEFT_HANDLING && instance->current_state != STATE_ROUNDRIGHT_HANDLING) instance->road_state = STRAIGHT_ROAD;
    else instance->road_state = BEND_ROAD;
    straight_left = is_straight_left;
    straight_right = is_straight_right;
    left_count_display = final_left_count;
    right_count_display = final_right_count;
    // 元素判定
    int corner_wid;
    if (instance->is_right_corner_locked && instance->is_left_corner_locked) {
        corner_wid = abs(left_line[left_lock_idx].x - right_line[right_lock_idx].x);
    }
    int lock_point_dif = abs(final_right_count - final_left_count);
    if (instance->current_state == STATE_NORMAL_TRACKING){//  && centerline_width_count < 18){
        if (check_zebra_crossing_at_line(binary_image, real_zebra_distance, 1, 12)) instance->current_state = STATE_STOP_HANDLING;
        if (instance->is_right_corner_locked && instance->is_left_corner_locked
                && right_corner_direction == CORNER_IN_UP_OUT_LEFT && left_corner_direction == CORNER_IN_UP_OUT_RIGHT && lock_point_dif > 6 && (final_left_count + final_right_count) < 50  && centerline_width_count < 18)
            instance->current_state = STATE_BRIDGE_HANDLING;
        else if (instance->is_right_corner_locked && instance->is_left_corner_locked
                && right_corner_direction == CORNER_IN_UP_OUT_LEFT && left_corner_direction == CORNER_IN_UP_OUT_RIGHT && lock_point_dif <= 6 && corner_wid < 55 && (final_left_count + final_right_count) < tiaoyuedian  && centerline_width_count < 18)
            instance->current_state = STATE_BARRIER_HANDLING;
        else if ((instance->is_right_corner_locked && instance->is_left_corner_locked) && (final_left_count + final_right_count) < 18 )
            instance->current_state = STATE_INTERSECTION_HANDLING;
        else if ((instance->is_right_corner_locked && is_straight_left && right_corner_direction == CORNER_IN_UP_OUT_RIGHT) && final_right_count < 8)
            instance->current_state = STATE_ROUNDRIGHT_HANDLING;
        else if ((instance->is_left_corner_locked && is_straight_right && left_corner_direction == CORNER_IN_UP_OUT_LEFT) && final_left_count < 8)
            instance->current_state = STATE_ROUNDLEFT_HANDLING;
        else if (((instance->is_left_corner_locked && is_straight_right && left_corner_direction == CORNER_IN_UP_OUT_RIGHT) && final_left_count < 9)
                        || ((instance->is_right_corner_locked && is_straight_left && right_corner_direction == CORNER_IN_UP_OUT_LEFT) && final_right_count < 9))
            instance->current_state = STATE_ROUNDLEFT_SMBA;
    }

    if (instance->current_state == STATE_BRIDGE_HANDLING){
        if (left_corner_direction == CORNER_IN_RIGHT_OUT_UP || left_corner_direction == CORNER_IN_LEFT_OUT_UP) final_left_count = 0;
        if (right_corner_direction == CORNER_IN_LEFT_OUT_UP || right_corner_direction == CORNER_IN_RIGHT_OUT_UP) final_right_count = 0;
    }

    runstate_display = instance->current_state;
    // 单边巡线锁定
    if (instance->centerline == CENTERLINE_LEFT) final_right_count = 0;
    else if (instance->centerline == CENTERLINE_RIGHT) final_left_count = 0;
    // --- 步骤 4: 根据（可能被截断后的）边线有效性，选择循迹模式 ---
    // 注意：这里使用 final_*_count
    bool is_left_valid = (final_left_count >= MIN_VALID_POINTS);
    bool is_right_valid = (final_right_count >= MIN_VALID_POINTS);
    centerline_width_count = 0;
    if (is_left_valid && is_right_valid) {
        return GenerateCenterline_Dual(instance, left_line, final_left_count, right_line, final_right_count, out_centerline);
    } else if (is_left_valid && !is_right_valid) {
        return GenerateCenterline_Single(instance, left_line, final_left_count, true, out_centerline);
    } else if (!is_left_valid && is_right_valid) {
        return GenerateCenterline_Single(instance, right_line, final_right_count, false, out_centerline);
    } else {
        return 0;
    }
}

// 纯跟踪部分
void PurePursuit_Init(PurePursuitController* controller, float track_width_m, float k_v, float ld_min_m) {
    controller->wheelbase_m = track_width_m; // 对于差速模型，这里存轮距T
    controller->K_v = k_v;
    controller->Ld_min_m = ld_min_m;
}

void PurePursuit_Update_Final(PurePursuitController* controller, Point* path, int point_count, Point car_pos, float current_speed_mps) {
    if (point_count == 0) return;

    Point_f path_vehicle_meters[240];
    for (int i = 0; i < point_count; ++i) {
        float vx_unit = (float)car_pos.y - path[i].y;
        float vy_unit = (float)car_pos.x - path[i].x;
        path_vehicle_meters[i].x = vx_unit * METERS_PER_UNIT;
        path_vehicle_meters[i].y = vy_unit * METERS_PER_UNIT;
    }

    float Ld = controller->K_v * current_speed_mps + controller->Ld_min_m;
    Point_f target_point_m = {Ld, 0}; // 默认目标点在正前方
    bool target_found = false;

    for (int i = 0; i < point_count; ++i) {
        if (path_vehicle_meters[i].x > 0) {
            float dist_to_target_m = hypotf(path_vehicle_meters[i].x, path_vehicle_meters[i].y);
            if (dist_to_target_m > Ld) {
                target_point_m = path_vehicle_meters[i];
                target_found = true;
                break;
            }
        }
    }

    if (!target_found) {
        int farthest_idx = -1;
        float max_dist_x_m = 0.0f;
        for (int i = 0; i < point_count; ++i) {
            if (path_vehicle_meters[i].x > max_dist_x_m) {
                max_dist_x_m = path_vehicle_meters[i].x;
                farthest_idx = i;
            }
        }
        if (farthest_idx != -1) {
            target_point_m = path_vehicle_meters[farthest_idx];
        } else {
            turning_radius_m = 0.0f;
            return;
        }
    }

    float xt_m = target_point_m.x;
    float yt_m = target_point_m.y;
    if (fabsf(yt_m) < 1e-6){
        turning_radius_m = 0.0f;
        return;
    }
    float radius_m = (xt_m * xt_m + yt_m * yt_m) / (2.0f * yt_m);
    if (fabsf(radius_m) < 80.0f) turning_radius_m = current_speed_mps / radius_m;
    else turning_radius_m = 0.0f;
}


//================================================================
// 私有辅助函数定义
//================================================================

/**
* @brief 使用Bresenham算法在查找表上绘制一条线
* @param map 要绘制的目标地图 (lookup_map)
* @param x0, y0 起始点坐标
* @param x1, y1 结束点坐标
*/
static void DrawLineOnMap(bool map[IMG_HEIGHT][IMG_WIDTH], int x0, int y0, int x1, int y1)
{
    // 计算位移差
    int dx = abs(x1 - x0);
    int dy = -abs(y1 - y0); // 使用-dy是为了让决策变量err的更新更简洁

    // 确定步进方向
    int sx = (x0 < x1) ? 1 : -1;
    int sy = (y0 < y1) ? 1 : -1;

    // 初始化决策变量
    int err = dx + dy;
    int e2;

    while (1) {
        // 1. 在当前点(x0, y0)做标记
        if (x0 >= 0 && x0 < IMG_WIDTH && y0 >= 0 && y0 < IMG_HEIGHT) {
            map[y0][x0] = true;
        }

        // 2. 检查是否到达终点
        if (x0 == x1 && y0 == y1) {
            break;
        }

        // 3. 计算下一个点的位置
        e2 = 2 * err;
        if (e2 >= dy) { // 如果 e2 >= dy，说明是时候在x方向上步进了
            err += dy;
            x0 += sx;
        }
        if (e2 <= dx) { // 如果 e2 <= dx，说明是时候在y方向上步进了
            err += dx;
            y0 += sy;
        }
    }
}

static void update_short_line_lookup_map(CenterlineGenerator* instance, Point* short_line, int short_count)
{
    // 清空上次的地图
    memset(instance->short_line_lookup_map, 0, sizeof(instance->short_line_lookup_map));
    // 标记新的短边线点
    if (short_count < 2) {
        // 如果点数少于2，无法连线，退化为只标记单个点
        if (short_count == 1) {
            Point p = short_line[0];
            if (p.x >= 0 && p.x < IMG_WIDTH && p.y >= 0 && p.y < IMG_HEIGHT) {
                instance->short_line_lookup_map[p.y][p.x] = true;
            }
        }
        return;
    }

    // 2. 遍历所有点对，并连接它们
    for (int i = 0; i < short_count - 1; ++i) {
        Point p1 = short_line[i];
        Point p2 = short_line[i + 1];

        DrawLineOnMap(instance->short_line_lookup_map, p1.x, p1.y, p2.x, p2.y);
    }
}


static void update_width_filter(CenterlineGenerator* instance, float new_width)
{
    instance->width_buffer[instance->width_buffer_index] = new_width;
    instance->width_buffer_index = (instance->width_buffer_index + 1) % WIDTH_FILTER_SIZE;

    float sum = 0;
    for (int i = 0; i < WIDTH_FILTER_SIZE; ++i) {
        sum += instance->width_buffer[i];
    }
    instance->filtered_width = sum / WIDTH_FILTER_SIZE;
}

/**
 * @brief [私有] 单边循迹模式：当只有一条边线时，使用“几何+参考”法线法计算中线。
 * @param instance CenterlineGenerator 实例，用于获取赛道宽度等信息。
 * @param single_line 单边线的点集数组。
 * @param line_count 单边线的点数量。
 * @param is_left_line 标志位，true表示输入的是左边线，false表示右边线。
 * @param out_centerline 用于存储生成的中线点集的数组。
 * @return 生成的中线点的数量。
 */
static int GenerateCenterline_Single(
    CenterlineGenerator* instance,
    Point* single_line, int line_count,
    bool is_left_line,
    Point* out_centerline
)
{
    int centerline_count = 0;
    // 使用最后一次可靠的赛道宽度的一半作为偏移量
    float offset = instance->filtered_width / 2.0f;

    // --- 循环起始点调整 ---
    // 由于新算法需要访问 i-2*RATE, i-RATE, i, i+RATE 等点，
    // 循环必须从一个安全的索引开始，以避免数组越界。
    const int start_index = 2 * CENTERLINE_DOWNSAMPLE_RATE;
    if (line_count < start_index + CENTERLINE_DOWNSAMPLE_RATE) {
        // 如果点数太少，无法应用此算法，直接返回
        return 0;
    }

    for (int i = start_index; i < line_count; i += CENTERLINE_DOWNSAMPLE_RATE)
    {
        Point_f final_normal; // 这是我们最终要使用的、已归一化的法线

        // --- 步骤1: 计算当前点的“几何法线” (方向精确，但朝向可能错误) ---
        // 使用中心差分法计算切线，更加精确
        int prev_idx = i - CENTERLINE_DOWNSAMPLE_RATE;
        int next_idx = i + CENTERLINE_DOWNSAMPLE_RATE;
        // 保护上边界
        if (next_idx >= line_count) {
            next_idx = line_count - 1;
        }

        Point_f current_tangent = {
            (float)(single_line[next_idx].x - single_line[prev_idx].x),
            (float)(single_line[next_idx].y - single_line[prev_idx].y)
        };

        // 通过将切线旋转90度得到法线
        Point_f geometric_normal = { current_tangent.y, -current_tangent.x };

        // --- 步骤2: 计算“参考法线”作为指南针 (基于历史点，朝向稳定) ---
        // 使用滞后点计算切线，作为方向参考
        int p1_idx = i - CENTERLINE_DOWNSAMPLE_RATE;
        int p2_idx = i - 2 * CENTERLINE_DOWNSAMPLE_RATE;

        Point_f lagging_tangent = {
            (float)(single_line[p1_idx].x - single_line[p2_idx].x),
            (float)(single_line[p1_idx].y - single_line[p2_idx].y)
        };

        Point_f reference_normal;
        // 根据是左线还是右线，确定参考法线必然指向赛道内侧
        if (is_left_line) { // 左线，法线应指向右侧
            reference_normal.x = lagging_tangent.y;
            reference_normal.y = -lagging_tangent.x;
        } else { // 右线，法线应指向左侧
            reference_normal.x = -lagging_tangent.y;
            reference_normal.y = lagging_tangent.x;
        }

        // --- 步骤3: 点乘校准，决定最终法线的朝向 ---
        // 计算几何法线和参考法线的点积
        float dot_product = geometric_normal.x * reference_normal.x + geometric_normal.y * reference_normal.y;

        // 如果点积大于等于0，说明两者方向大致相同（夹角小于90度），几何法线朝向正确
        // 如果点积小于0，说明两者方向大致相反（夹角大于90度），需要翻转几何法线
        if (dot_product <= 0) {
            final_normal = geometric_normal;
        } else {
            final_normal.x = -geometric_normal.x;
            final_normal.y = -geometric_normal.y;
        }
        
        // --- 步骤4: 归一化最终法线 ---
        float mag = hypotf(final_normal.x, final_normal.y);
        if (mag > 1e-6) { // 避免除以零
            final_normal.x /= mag;
            final_normal.y /= mag;
        } else {
            // 如果法线向量长度过小（例如，多个点重合），则跳过此点
            continue;
        }

        // --- 步骤5: 生成中线点 ---
        // 沿最终确定的法线方向，偏移半个赛道宽度
        Point current_point = single_line[i];
        Point center_point;
        center_point.x = (int)roundf(current_point.x + offset * final_normal.x);
        center_point.y = (int)roundf(current_point.y + offset * final_normal.y);

        out_centerline[centerline_count++] = center_point;
    }

    return centerline_count;
}

/**
 * @brief [私有] 双边循迹模式：当两条边线都存在时，计算中线
 */
static int GenerateCenterline_Dual(
    CenterlineGenerator* instance,
    Point* left_line, int left_count,
    Point* right_line, int right_count,
    Point* out_centerline
)
{
    // --- 步骤1: 检查输入有效性并确定基准边线 ---
    // 拟合至少需要 2*N+1 个点
    if (left_count < (2 * NORMAL_FIT_NEIGHBORHOOD + 1) || right_count < (2 * NORMAL_FIT_NEIGHBORHOOD + 1)) {
        return 0; // 点数过少，无法进行稳定计算
    }

    Point* long_line = (left_count > right_count) ? left_line : right_line;
    int long_count = (left_count > right_count) ? left_count : right_count;
    Point* short_line = (left_count > right_count) ? right_line : left_line;
    int short_count = (left_count > right_count) ? right_count : left_count;

    bool is_long_line_left = (long_line == left_line);

    // --- 步骤2: 更新短边线查找表以备高效搜索 ---
    update_short_line_lookup_map(instance, short_line, short_count);

    // --- 步骤3: 为混合决策策略预计算所需信息 ---
    int centerline_count = 0; // 初始化中线点计数器

    // --- 步骤4: 降采样遍历基准边线 ---
    for (int i = 0; i < long_count; i += CENTERLINE_DOWNSAMPLE_RATE)
    {
    // 如果无法回顾前两个点，则直接跳过，不处理此点
    if (i < 2 * CENTERLINE_DOWNSAMPLE_RATE) {
        continue;
    }

    Point current_point = long_line[i];
    Point_f final_normal; // 这是我们最终要使用的法线

    // --- 步骤1: 计算当前点的“几何法线” (方向精确，但朝向可能错误) ---
    // 此处 i-1 和 i+1 索引必然有效
    int prev_idx = i - CENTERLINE_DOWNSAMPLE_RATE;
    int next_idx = i + CENTERLINE_DOWNSAMPLE_RATE;
    if (next_idx >= long_count) next_idx = long_count - 1; // 仅需保护上界

    Point_f current_tangent = {
        (float)(long_line[next_idx].x - long_line[prev_idx].x),
        (float)(long_line[next_idx].y - long_line[prev_idx].y)
    };

    Point_f geometric_normal;
    geometric_normal.x = current_tangent.y;
    geometric_normal.y = -current_tangent.x;

    // --- 步骤2: 计算“参考法线”作为指南针 (朝向稳定) ---
    // 此处 i-1 和 i-2 索引必然有效
    int p1_idx = i - CENTERLINE_DOWNSAMPLE_RATE;
    int p2_idx = i - 2 * CENTERLINE_DOWNSAMPLE_RATE;

    Point_f lagging_tangent = {
        (float)(long_line[p1_idx].x - long_line[p2_idx].x),
        (float)(long_line[p1_idx].y - long_line[p2_idx].y)
    };

    Point_f reference_normal;
    if (is_long_line_left) { // 左线，参考法线向右偏
        reference_normal.x = lagging_tangent.y;
        reference_normal.y = -lagging_tangent.x;
    } else { // 右线，参考法线向左偏
        reference_normal.x = -lagging_tangent.y;
        reference_normal.y = lagging_tangent.x;
    }

    // --- 步骤3: 点乘校准，决定最终法线的朝向 ---
    float dot_product = geometric_normal.x * reference_normal.x + geometric_normal.y * reference_normal.y;

    if (dot_product <= 0) {
        // 方向一致，几何法线的朝向是正确的
        final_normal = geometric_normal;
    } else {
        // 方向相反，需要将几何法线翻转180度
        final_normal.x = -geometric_normal.x;
        final_normal.y = -geometric_normal.y;
    }

    // 归一化最终的法线
    float mag = hypotf(final_normal.x, final_normal.y);
    if (mag > 1e-6) {
        final_normal.x /= mag;
        final_normal.y /= mag;
    }

        // --- 步骤6: 沿法线搜索对应点 ---
        bool found_match = false;
        Point matched_point = {0, 0};
        float max_search_dist = instance->filtered_width * MAX_SEARCH_FACTOR;

        for (float d = 0; d < max_search_dist; d += 1.0f) {
            int search_x = (int)(current_point.x + d * final_normal.x);
            int search_y = (int)(current_point.y + d * final_normal.y);

            if (search_x >= 0 && search_x < IMG_WIDTH && search_y >= 0 && search_y < IMG_HEIGHT) {
                if (instance->short_line_lookup_map[search_y][search_x]) {
                    found_match = true;
                    matched_point.x = search_x;
                    matched_point.y = search_y;
                    break;
                }
            } else {
                break;
            }
        }

        // --- 步骤7: 计算中线点并更新宽度 ---
        if (found_match) {
            // --- A. 匹配成功的分支 ---
            float center_x_f = ((float)current_point.x + (float)matched_point.x) / 2.0f;
            float center_y_f = ((float)current_point.y + (float)matched_point.y) / 2.0f;

            // 【修改点 2】将浮点结果通过“加0.5再取整”的方式进行四舍五入，然后存入输出数组。
            // 这比直接 (int) 强制转换（截断）更精确。
            out_centerline[centerline_count].x = (int)(center_x_f + 0.5f);
            out_centerline[centerline_count].y = (int)(center_y_f + 0.5f);

            // 宽度计算和滤波部分保持不变
            float new_width = hypotf((float)current_point.x - matched_point.x, (float)current_point.y - matched_point.y);
            if (new_width < 45) new_width = 45;
            update_width_filter(instance, new_width);
        } else {
            // --- B. 匹配失败的分支 ---
            float offset = instance->filtered_width / 2.0f;

            // 计算浮点坐标
            float center_x_f = (float)current_point.x + offset * final_normal.x;
            float center_y_f = (float)current_point.y + offset * final_normal.y;
            centerline_width_count++;
            // 【修改点 3】同样使用四舍五入来提高精度，避免因截断导致的位置跳变。
            out_centerline[centerline_count].x = (int)(center_x_f + 0.5f);
            out_centerline[centerline_count].y = (int)(center_y_f + 0.5f);
        }

        // --- 步骤8: 存储结果并递增计数器 ---
        centerline_count++;
    }

    return centerline_count;
}

/**
 * @brief (精确点检测) 判断指定索引处的点是否为一个直角弯。
 * @param line              [in]  输入的路径点序列数组 (中线或边线)
 * @param point_count       [in]  路径点的总数量
 * @param target_idx        [in]  要检查的目标点的索引
 * @param k                 [in]  计算角度时选取的邻域步长
 * @param cos_theta_thresh  [in]  判断直角的余弦阈值
 * @return int              1表示是直角，0表示不是。
 */
int is_right_angle_at_point(
    const Point* line,
    int point_count,
    int target_idx,
    int k,
    float cos_theta_thresh_sq)
{
    if (line == NULL || target_idx < k || target_idx >= point_count - k) {
        return 0;
    }

    const Point p_i = line[target_idx];
    const Point p_prev = line[target_idx - k];
    const Point p_next = line[target_idx + k];

    float v1_x = (float)p_prev.x - p_i.x;
    float v1_y = (float)p_prev.y - p_i.y;
    float v2_x = (float)p_next.x - p_i.x;
    float v2_y = (float)p_next.y - p_i.y;

    float dot_product = v1_x * v2_x + v1_y * v2_y;
    float mag1_sq = v1_x * v1_x + v1_y * v1_y; // 幅值的平方
    float mag2_sq = v2_x * v2_x + v2_y * v2_y; // 幅值的平方

    // 避免除以零，并确保向量长度有意义
    if (mag1_sq > 1e-6f && mag2_sq > 1e-6f) {
        // 核心判断： (dot)² < thresh² * mag1² * mag2²
        if (dot_product * dot_product < cos_theta_thresh_sq * mag1_sq * mag2_sq) {
            return 1; // 是直角！
        }
    }
    return 0;
}

/**
 * @brief (路径扫描) 从指定索引开始，寻找路径中第一个直角拐点的索引。
 * @param start_idx         [in]  从路径的哪个索引开始搜索，避免重复检测已通过的拐点。
 * @return int              返回第一个检测到的拐点的索引。如果没找到，则返回-1。
 */
int find_first_corner_idx(
    const Point* line,
    int point_count,
    int start_idx,
    int k,
    float cos_theta_thresh)
{
    // 从 start_idx 开始遍历路径
    for (int i = start_idx; i < point_count; ++i) {
        // 在每个点上，调用我们的单点检测函数
        if (is_right_angle_at_point(line, point_count, i, k, cos_theta_thresh)) {
            // 找到了！立即返回这个点的索引
            return i;
        }
    }

    // 遍历完都没找到
    return -1;
}

/**
 * @brief 使用首尾两点定义基准线，通过计算均方误差(MSE)来判断路径是否为直线。
 * @param line          [in] 输入的路径点Point结构体数组
 * @param num           [in] 路径点的总数量
 * @param mse_thresh    [in] 判断直线所用的MSE阈值
 * @return bool         true表示是直线，false表示是弯道。
 */
bool is_line_straight_by_endpoints(
    const Point* line,
    int num,
    int minnum,
    int maxnum,
    float mse_thresh)
{
    // 如果点数太少，无法进行有意义的判断，直接返回false
    if (num < minnum) {
        return false;
    }
    if (num > maxnum) num = maxnum;

    // 1. 选择两个参考点来定义基准线
    // 注意：我们将int类型的坐标转换为float来进行精确计算
    float x1 = (float)line[6].x;
    float y1 = (float)line[6].y;
    float x2 = (float)line[num - 7].x;
    float y2 = (float)line[num - 7].y;

    // 2. 使用标准、安全的直线方程 Ax + By + C = 0 计算参数
    // A = y2 - y1
    // B = x1 - x2
    // C = -A*x1 - B*y1
    float A = y2 - y1;
    float B = x1 - x2;
    float C = -A * x1 - B * y1;

    float den_sq = A * A + B * B;
    // 如果两个参考点重合，无法定义直线
    if (den_sq < 1e-6f) {
        return true; // 点都挤在一起，也算一种“直线”
    }

    // 3. 计算中间点到基准线的均方误差(MSE)
    float sum_sq_dist = 0.0f;
    int inner_points_count = num - 6;

    for (int i = 3; i < num - 3; i++)
    {
        // 同样，将int坐标转为float
        float x0 = (float)line[i].x;
        float y0 = (float)line[i].y;

        // 点到直线的距离公式的平方: d² = (Ax₀+By₀+C)² / (A²+B²)
        float dist_sq = (A * x0 + B * y0 + C) * (A * x0 + B * y0 + C) / den_sq;
        sum_sq_dist += dist_sq;
    }

    // 检查是否有有效的内点
    if (inner_points_count <= 0) {
        return true; // 没有内点可供判断，认为其是直线
    }

    float mse = sum_sq_dist / inner_points_count;

    // 4. 比较MSE与阈值，做出最终判断
    if (mse <= mse_thresh) {
        return true; // 误差很小，判定为直线
    } else {
        return false; // 误差较大，判定为弯道
    }
}

/**
 * @brief 获取一个向量的主方向 (上/下/左/右)
 * @param vx 向量的x分量
 * @param vy 向量的y分量
 * @return VectorDirection 向量的主方向
 */
static VectorDirection get_vector_direction(float vx, float vy) {
    if (fabsf(vy) > fabsf(vx)) {
        return (vy > 0) ? DIR_DOWN : DIR_UP;
    } else {
        return (vx > 0) ? DIR_RIGHT : DIR_LEFT;
    }
}

/**
 * @brief (新方法) 通过判断入弯和出弯向量的主方向来确定拐角类型
 * @param line               [in] 输入的路径点序列数组
 * @param point_count        [in] 路径点的总数量
 * @param target_idx         [in] 要检查的目标点的索引
 * @param k                  [in] 邻域步长
 * @param cos_theta_thresh_sq [in] 判断直角的余弦阈值的平方
 * @return CornerDirection   返回检测到的拐角方向，如果不是则返回 NO_CORNER。
 */
CornerDirection detect_corner_by_vector_direction(
    const Point* line,
    int point_count,
    int target_idx,
    int k,
    float cos_theta_thresh_sq)
{
    if (line == NULL || target_idx < k || target_idx >= point_count - k) {
        return NO_CORNER;
    }

    const Point p_i = line[target_idx];
    const Point p_prev = line[target_idx - k];
    const Point p_next = line[target_idx + k];

    // --- 第一步：确认是拐角 ---
    // 依然使用高效的点积法来确认角度是否接近90度
    float v1_x = (float)p_i.x - p_prev.x;
    float v1_y = (float)p_i.y - p_prev.y;
    float v2_x = (float)p_i.x - p_next.x;
    float v2_y = (float)p_i.y - p_next.y;

    float dot_product = v1_x * v2_x + v1_y * v2_y;
    float mag1_sq = v1_x * v1_x + v1_y * v1_y;
    float mag2_sq = v2_x * v2_x + v2_y * v2_y;

    if (mag1_sq < 1e-6f || mag2_sq < 1e-6f ||
        dot_product * dot_product >= cos_theta_thresh_sq * mag1_sq * mag2_sq) {
        return NO_CORNER; // 不是直角拐角
    }

    // --- 第二步：判断方向 ---
    // vec_in:  p_prev -> p_i
    // vec_out: p_i -> p_next
    float vec_in_x = (float)p_i.x - p_prev.x;
    float vec_in_y = (float)p_i.y - p_prev.y;
    float vec_out_x = (float)p_next.x - p_i.x;
    float vec_out_y = (float)p_next.y - p_i.y;

    VectorDirection dir_in = get_vector_direction(vec_in_x, vec_in_y);
    VectorDirection dir_out = get_vector_direction(vec_out_x, vec_out_y);

    // 根据上文的映射表进行判断
    // --- 分开判断的逻辑 ---
    if (dir_in == DIR_UP && dir_out == DIR_RIGHT) return CORNER_IN_UP_OUT_RIGHT;
    if (dir_in == DIR_LEFT && dir_out == DIR_UP) return CORNER_IN_LEFT_OUT_UP;

    if (dir_in == DIR_UP && dir_out == DIR_LEFT) return CORNER_IN_UP_OUT_LEFT;
    if (dir_in == DIR_RIGHT && dir_out == DIR_UP) return CORNER_IN_RIGHT_OUT_UP;

    if (dir_in == DIR_DOWN && dir_out == DIR_LEFT) return CORNER_IN_DOWN_OUT_LEFT;
    if (dir_in == DIR_RIGHT && dir_out == DIR_DOWN) return CORNER_IN_RIGHT_OUT_DOWN;

    if (dir_in == DIR_DOWN && dir_out == DIR_RIGHT) return CORNER_IN_DOWN_OUT_RIGHT;
    if (dir_in == DIR_LEFT && dir_out == DIR_DOWN) return CORNER_IN_LEFT_OUT_DOWN;

    return NO_CORNER; // 理论上，如果通过了角度检查，不会到达这里
}


/**
 * @brief (路径扫描) 从指定索引开始，寻找路径中第一个直角拐点，并返回其方向和索引。
 *
 * @param line              [in]  输入的路径点序列数组
 * @param point_count       [in]  路径点的总数量
 * @param start_idx         [in]  从路径的哪个索引开始搜索
 * @param k                 [in]  计算角度时选取的邻域步长
 * @param cos_theta_thresh  [in]  判断直角的余弦阈值 (注意：非平方值)
 * @param out_corner_idx    [out] 用于存储找到的拐点索引的指针。如果未找到，将被设为-1。
 * * @return CornerDirection  返回第一个检测到的拐点的方向。如果没找到，则返回 NO_CORNER。
 */
CornerDirection find_first_corner(
    const Point* line,
    int point_count,
    int start_idx,
    int k,
    float cos_theta_thresh,
    int* out_corner_idx)
{
    // 在循环外计算一次阈值的平方，提高效率
    const float cos_theta_thresh_sq = cos_theta_thresh * cos_theta_thresh;

    if (point_count > 50) point_count = 50;

    // 从 start_idx 开始遍历路径
    for (int i = start_idx; i < point_count; ++i) {

        // --- 核心改动：调用新的检测函数 ---
        // 这里我们调用上一问中逻辑最清晰的 `detect_corner_by_vector_direction`
        CornerDirection direction = detect_corner_by_vector_direction(
            line, point_count, i, k, cos_theta_thresh_sq
        );

        // --- 核心改动：检查返回值 ---
        // 如果返回的不是 NO_CORNER，说明找到了
        if (direction != NO_CORNER) {
            // 找到了！通过指针返回这个点的索引
            if (out_corner_idx != NULL) {
                *out_corner_idx = i;
            }
            // 将检测到的方向作为函数返回值
            return direction;
        }
    }

    // 遍历完都没找到
    if (out_corner_idx != NULL) {
        *out_corner_idx = -1;
    }
    return NO_CORNER;
}

/**
 * @brief (基础工具函数) 从左到右扫描单行，计算黑白跳变点数量。
 */
int count_row_transitions(const uint8_t* row_ptr, int image_width) {
    if (image_width < 2) return 0;
    int transition_count = 0;
    for (int i = left_zebra.x; i < right_zebra.x; ++i) {
        if ((row_ptr[i] == row_ptr[i-1]) && (row_ptr[i-2] == row_ptr[i-3]) && (row_ptr[i-1] != row_ptr[i-2])) {
            transition_count++;
        }
    }
    return transition_count;
}

/**
 * @brief 在指定中心行的上下进行三点采样，判断是否为斑马线。
 *
 * @param binarized_image   完整的120x188二值化图像数组。
 * @param center_row        采样的中心行号。
 * @param row_offset        上下偏移量（例如，传入3，则检测 center_row-3, center_row, center_row+3）。
 * @param threshold         判定为斑马线的跳变次数阈值。
 *
 * @return int              如果三行都满足条件，返回1 (true)；否则返回0 (false)。
 */
int check_zebra_crossing_at_line(
    const uint8_t binarized_image[IMAGE_HEIGHT][IMAGE_WIDTH],
    int center_row,
    int row_offset,
    int threshold
) {
    // 1. 定义需要检查的三行
    int rows_to_check[5] = {
        center_row - row_offset * 2,
        center_row - row_offset,
        center_row,
        center_row + row_offset,
        center_row + row_offset * 2
    };

    // 2. 遍历这6行进行检查
    for (int i = 0; i < 5; i++) {
        int current_row = rows_to_check[i];

        // --- 边界安全检查，非常重要！ ---
        if (current_row < 0 || current_row >= IMAGE_HEIGHT) {
            // 如果要检查的行超出了图像边界，直接认为检测失败
            return 0;
        }

        // 获取该行的跳变次数
        int transitions = count_row_transitions(binarized_image[current_row], IMAGE_WIDTH);

        // --- 核心判断逻辑 ---
        // 只要有一行的跳变数不满足阈值，就立即返回失败
        if (transitions < threshold) {
            return 0;
        }
    }

    // 3. 如果循环能顺利结束，说明三行全部满足条件
    return 1; // 确认检测到斑马线
}
